#include "pca9685.h"
#include "iic.h"
#include "math.h"

/**
 * @brief 向PCA写数据
 * @param adrrd地址 data数据
 */
void pca_write(u8 adrr,u8 data)
{ 
	I2C_Start(IIC1);
	
	I2C_SendByte(IIC1,pca_adrr);
	I2C_WaitAck(IIC1);
	
	I2C_SendByte(IIC1,adrr);
	I2C_WaitAck(IIC1);
	
	I2C_SendByte(IIC1,data);
	I2C_WaitAck(IIC1);
	
	I2C_Stop(IIC1);
}

/**
 * @brief 从PCA读数据
 * @param 地址
 */
u8 pca_read(u8 adrr)
{
	u8 data;
	I2C_Start(IIC1);
	
	I2C_SendByte(IIC1,pca_adrr);
	I2C_WaitAck(IIC1);
	
	I2C_SendByte(IIC1,adrr);
	I2C_WaitAck(IIC1);
	
	I2C_Start(IIC1);
	
	I2C_SendByte(IIC1,pca_adrr|0x01);
	I2C_WaitAck(IIC1);
	
	data = I2C_ReadByte(IIC1);
	I2C_Stop(IIC1);
	
	return data;
}


/**
 * @brief PCA复位
 */
void pca_reset(void)
{
	IIC_Init(IIC1);
	pca_write(pca_mode1,0x0);
}


/**
 * @brief 设置pca的PWM频率
 * @param freq:pwm频率
 */
void pca_setfreq(float freq)
{
		u8 prescale,oldmode,newmode;
		double prescaleval,freq_d;

		freq_d = freq;
		freq_d *= 0.92; 

		prescaleval = (25000000/4096/freq_d)-1;

		prescale = floor(prescaleval + 0.5f);
		oldmode = pca_read(pca_mode1);
		newmode = (oldmode&0x7F) | 0x10; //准备进入sleep以设置时钟
	
		pca_write(pca_mode1, newmode); // go to sleep
		pca_write(pca_pre, prescale); // set the prescaler
		pca_write(pca_mode1, oldmode);

		delayMs(2);
	
		pca_write(pca_mode1, oldmode | 0xa1); 
}


/**
 * @brief 	设置pca的PWM占空比
 * @param 	id:PWM输出引脚0~15
 * 			on:PWM上升计数值
 * 			off:PWM下降计数值
 * @note  	PWM周期为4096 
 * 			由0开始+1计数，计到on时跳变为高电平，继续计数到off时，跳变为低电平，直到计满4096重新开始。
 * 			所以当 on 不等于0时可作延时,当on等于0时，off/4096的值就是PWM的占空比。
 *			当on为0时，0ff / 4096 的值就为占空比
 */
void pca_setpwm(u8 id, u32 on, u32 off)
{
	pca_write(LED0_ON_L  + 4*id, on);
	pca_write(LED0_ON_H  + 4*id, on>>8);
	pca_write(LED0_OFF_L + 4*id,off);	
	pca_write(LED0_OFF_H + 4*id,off>>8);
}

/**
 * @brief 	pca的pwm输出设置
 * @param 	id:PWM输出引脚0~15
 * 			pwm:PWM占空比*4096
 * @note  	0<=pwm<=4096
 */
void pca_pwmout(u8 id, u32 CCR)
{
	pca_setpwm(id,0,CCR);
}


// /**
//  *	@brief 初始化舵机驱动板
//  *	@param PWM频率 初始化舵机角度
//  *	@note  0-15通道全部设置为0，设置舵机频率以及初始角度
//  **/
// void Servo_Init(float hz,u8 angle)
// {
//     u32 off=0;                    //占空比为0
//     pca_write(pca_mode1,0x0);
//     pca_setfreq(hz);              //设置PWM频率
//     off=(u32)(145+angle*2.4);

//     pca_setpwm(0,0,off);
//     pca_setpwm(1,0,off);
//     pca_setpwm(2,0,off);
//     pca_setpwm(3,0,off);
//     pca_setpwm(4,0,off);
//     pca_setpwm(5,0,off);
//     pca_setpwm(6,0,off);
//     pca_setpwm(7,0,off);
//     pca_setpwm(8,0,off);
//     pca_setpwm(9,0,off);
//     pca_setpwm(10,0,off);
//     pca_setpwm(11,0,off);
//     pca_setpwm(12,0,off);
//     pca_setpwm(13,0,off);
//     pca_setpwm(14,0,off);
//     pca_setpwm(15,0,off);

//     delayMs(500);
// }


// /**
//  *  @brief 控制舵机转动
//  *  @param id{pca9685 输出端口0-15) angle{角度，比如输入60，舵机则转动60度}
//  *  @note  设置舵机按照预设角度转动
// */
// void Servo_angle(u8 id,u8 angle)
// {
//     u32 off = 0;
//     off = (u32)(158+angle*2.2);
//     pca_setpwm(id,0,off);
// }

